/*
 * Quadcopter.cpp
 *
 *  Created on: 6 mei 2013
 *      Author: Wietse
 */

#include "Quadcopter.h"
#include "../Swarm.h"
#include "math.h"
#include <iostream>
#include "../Position.h"
#include "../Definitions.h"
#include <algorithm>
#include "../Data.h"
#include <iostream>
#include <fstream>

/**
 * Default constructor
 */
Quadcopter::Quadcopter()
{
	init();

	sourceType = QUADCOPTER;
	ID = 0;
	position = Position();
	velocity = Position(5.0,5.0);
	agent	= QAgent(&position);
	this->timer = 0.0;
}

/**
 * Preferred constructor
 */
Quadcopter::Quadcopter(int ID, Swarm *swarm, Position pos)
{
	init();

	sourceType = QUADCOPTER;
	this->ID = ID;
	position = pos;
	velocity = Position(5.0,5.0);
	/*
	//Get random doubles for x and z
	double f = (double)rand() / RAND_MAX;
	double xx = 0.0 + f * (100.0 - 0.0);

	f = (double)rand() / RAND_MAX;
	double zz = 0.0 + f * (100.0 - 0.0);

	target 	=	Position(xx,zz);
	 */
	agent	= QAgent(&position);

	this->timer = 0.0;
}

/**
 * Default destructor
 */
Quadcopter::~Quadcopter() {
}

/**
 * Updates several aspects of the Quadcopter
 */
void Quadcopter::update(Swarm *swarm, double deltaTime, Quadcopter *quadcopters)
{
	// Update the position of the quadcopter
	updatePosition(swarm, deltaTime, quadcopters);

	// Detect the crawlers that are nearby
	senseCrawlers(swarm);

	// Receive messages
	receiveMessages(swarm);

	// Increment message_timer
	timer += deltaTime;

	// Check if the timer has expired
	if(timer > QUADCOPTER_MESSAGESPEED)
	{

		// Send message
		if(!RANDOM_MESSAGE)
		{
			sendMessage(swarm, deltaTime);
		}
		else
		{	//Random messages on
			if(rand() % 100 < SEND_PROBABILITY)
			{	//Send with probability SEND_PROBABILITY
				//std::cout << "Quadcopter " << this->ID << " sending messages randomly" << std::endl;
				sendMessage(swarm, deltaTime);		//Also sends data about crawlers (and stores it into its own data)
			}
		}

		// Print the received data
		if(DEBUG)
		{
			printData();
			printImage();
		}

		// Generate output to txt files
		if(OUTPUT)
		{
			outputData();
			outputImage();
			outputImageCounter();
		}

		//Remove expired data
		removeData();

		// Reset the message timer
		timer = 0;
	}
}

#ifdef SWARM_AGENT
/**
 * Update the position of the Quadcopter based on the target that is generated by the QAgent
 */
void Quadcopter::updatePosition(Swarm *swarm, double deltaTime, Quadcopter *quadcopters)
{
	// Calculate the target
	target = agent.calculateTarget(swarm, &position, &receivedData, deltaTime);

	double pullDistance = position.distance(&target);


	//This code decides the margin that is allowed to be away from the target.
	//If target is reached a bigger margin is allowed from the target, so the quadcopter won't move too much
	if(pullDistance < 1.0) {
		targetReached = true;
	}
	else if(pullDistance > 5.0) {
		targetReached = false;
	}

	double margin;

	if(targetReached) {
		margin = 5.0;
	}
	else  {
		margin = 1.0;
	}

	// Only do something if we are not already there
	if (pullDistance > margin)
	{
		Position pull = Position::sub(&target, &position);
		pull.div(pullDistance);	//the target tries to 'pull us in'

		Position totalPush = Position(0.0,0.0);

		int contenders = 0;
		for (int i = 0; i < NUM_OF_QUADCOPTERS; i++)
		{
			// Draw a vector from the obstacle to the quadcopter, that 'pushes the quadcopter away'
			Position push = Position::sub(&position, &quadcopters[i].position);

			// Calculate how much we are pushed away from this obstacle, the closer, the more push
			float distance = position.distance(&quadcopters[i].position) - QUADCOPTER_RADIUS - QUADCOPTER_RADIUS;

			// Only use push force if this object is close enough such that an effect is needed
			if (distance < QUADCOPTER_RADIUS*2 && distance !=0)
			{
				// Note that this object is actively pushing
				++contenders;

				// Prevent division by zero errors and extreme pushes
				if (distance < 0.0001f)
				{
					distance = 0.0001f;
				}
				double weight = 1 / distance;

				//totalPush += push * weight;
				push.mul(weight);

				totalPush.add(&push);
			}
		}

		// 4 * contenders gives the pull enough force to pull stuff trough
		pull.mul(std::max(1, 8 * contenders));
		pull.add(&totalPush);

		// Normalize the vector so that we get a vector that points in a certain direction, which we van multiply by our desired speed

		pull.setMag(QUADCOPTER_SPEED);

		// Set the quadcopter's new position;
		velocity.setX(pull.getX());
		velocity.setZ(pull.getZ());

		// Update the actual position of the quadcopter
		position.setX(position.getX() + velocity.getX()*deltaTime);
		position.setZ(position.getZ() + velocity.getZ()*deltaTime);
	}
}
#endif

#ifdef SOLO_AGENT
/**
 * Update the position of the Quadcopter based on the target that is generated by the QAgent
 */
void Quadcopter::updatePosition(Swarm *swarm, double deltaTime, Quadcopter *quadcopters)
{
	// Store the old position
	position.backup();

	// Calculate the target
	target = agent.calculateTarget(swarm, &position, &receivedData, deltaTime);

	// Calculate the distance to the target
	double distanceToTarget = position.distance(&target);

	// Check if we are not already close enough to the target
	// move to QAgent
	if (distanceToTarget > 3.0)
	{
		// Calculate the angle and the distance covered
		double angle = target.angle(&position);
		double distanceCovered = QUADCOPTER_SPEED*deltaTime;

		// Update the position
		position.addX(distanceCovered*cos(angle));
		position.addZ(distanceCovered*sin(angle));

		// Iterate over the quadcopter array
		for(int i = 0; i < NUM_OF_QUADCOPTERS; i++)
		{
			//std::cout << "for " << i << std::endl;
			int otherID = *(&quadcopters[i].ID);
			if(otherID != ID)
			{
				// Calculate the range between this quadcopter and the other quadcopter
				double range = position.distance(&quadcopters[i].position);

				// Check if the other quadcopter is to close to this quadcopter
				if(range < QUADCOPTER_SAFETY_MARGIN)
				{
					// Restore the old position
					position.restore();

					// Detect the angle between this and the other quadcopter
					angle = position.angle(&quadcopters[i].position);

					// Set the new position
					angle -= 45.0;
					position.addX(distanceCovered*cos(angle));
					position.addZ(distanceCovered*sin(angle));

					break;
				}
			}
		}
	}
}
#endif


/**
 * Check if there are Crawlers nearby the Quadcopter
 */
void Quadcopter::senseCrawlers(Swarm *swarm)
{
	// Delete the previous list of crawlers that were nearby the quadcopter
	nearbyCrawlers.clear();

	// Get a new list of crawlers that are nearby the quadcopter
	nearbyCrawlers = swarm->senseCrawlers(&position);
}

/**
 * Send a message about this quadcopter and the crawlers that are nearby
 */
void Quadcopter::sendMessage(Swarm *swarm, double deltaTime)
{
	// Retrieve the current time
	time_t t = time(0);
	struct tm now = *(localtime(&t));

	// Create the data and message object about this quadcopter and send it
	Data data = Data(QUADCOPTER, this->ID, this->position, &now);
	Message msg = Message(QUADCOPTER, this->ID, this->position, data);
	swarm->addMessage(msg);

	// Send data about Crawlers that are nearby
	for(std::vector<int>::size_type i = 0; i < nearbyCrawlers.size(); i++)
	{
		// Construct data and message object about the Crawler and send the message
		Data data = Data(CRAWLER, nearbyCrawlers[i]->getID(), *(nearbyCrawlers[i]->getPosition()), &now);
		addData(swarm, data, false);
		Message msg = Message(QUADCOPTER, this->ID, this->position, data);
		swarm->addMessage(msg);
	}
}

/**
 * Display the quadcopter on screen
 */
void Quadcopter::display()
{
	// Set the color for the quadcopter
	glColor3f(0.0,0.0,0.0);

	glBegin(GL_POLYGON);

	// Draw the vertices
	glVertex3f(position.getX()-2.0*QUADCOPTER_WIDTH/3.0, position.getZ()-QUADCOPTER_HEIGHT/3.0, 0.0);
	glVertex3f(position.getX()+2.0*QUADCOPTER_WIDTH/3.0, position.getZ()-QUADCOPTER_HEIGHT/3.0, 0.0);
	glVertex3f(position.getX(), position.getZ()+2.0*QUADCOPTER_HEIGHT/3.0, 0.0);

	glEnd();


	// Set the color for the target
	glColor3f(0.5,0.5,0.5);

	glBegin(GL_POLYGON);

	// Draw the vertices
	glVertex3f(target.getX()-2.0*QUADCOPTER_WIDTH/3.0, target.getZ()+QUADCOPTER_HEIGHT/3.0, 0.0);
	glVertex3f(target.getX()+2.0*QUADCOPTER_WIDTH/3.0, target.getZ()+QUADCOPTER_HEIGHT/3.0, 0.0);
	glVertex3f(target.getX(), target.getZ()-2.0*QUADCOPTER_HEIGHT/3.0, 0.0);

	glEnd();
}
